#include "bewegen.h"

#include <stdio.h>


/*
-------------------------------------------------------
Funktion "void bewegen.." führt die Bewegung des Roboters,
die der Werkzeuge sowie die der Bleche aus
-------------------------------------------------------
xe: x-Koordinate der Endstellung des Roboterarms
ye: y-Koordinate der Endstellung des Roboterarms
g: Greifervariable	*0: kein Werkzeug im Greifer
					*1: offene Hand
					*2: geschlossene Hand
					*3: Schweißgerät
xse: x-Koordinate der anzufahrenden Schienenposition
t_max: Zeitdauer der Roboterbewegung in [ms]
f1,f2: Fließbandvariablen	*0: Blech im Greifer
							*1: Blech auf Eingangsfließband
							*2: Blech auf Ausgangsfließband
					
*/

void bewegen(HWND hwnd, HDC hdc, RECT rect, double xe, double ye, int g, double xse, int t_max, int f1, int f2)
{
	int anz_loes;
	int i, t;
	int t_step = 40;			// dt in ms
	double xsa;				// x-Ausgangsposition des Roboters auf der Schiene
	double sollpunkt[2];
	double xa, ya;
	double winkel1[2], winkel2[2];
	double verschiebung[2];

	xa = robo2_n[9][0];		//Festsetzen von Anfangspostion des Roboters
	ya = robo2_n[9][1];

	xsa = robo1_n[4][0];	//Festsetzen von Schienenposition des Roboters


/*Roboterarmtransfomation:	*/

	for (t=t_step; t<=t_max; t=t+t_step)
	{
		sollpunkt[0]= xa + t*(xe-xa-(xse-xsa))/(t_max) - robo1[4][0] - xsa;
		sollpunkt[1]= ya + t*(ye-ya)/(t_max) - robo1[4][1] - 300;

		anz_loes=SuchenWinkel(sollpunkt, l, l, winkel1, winkel2);
		
		verschiebung[0]= (t * (xse-xsa) / t_max) + xsa;
		verschiebung[1]= 300;
 	 
		for (i=0;i<6;i++)		//Transformation 1. Arm
			TrafoPoint(robo1[i], robo1_n[i], winkel1[0]+180, robo1[4], verschiebung);



		for (i=0;i<10;i++)		//Transformation 2. Arm
			TrafoPoint(robo2[i], robo2_n[i],  winkel2[0]+180,  robo2[8],  robo1_n[5]);
		
		
		switch(g)			//Werkzeugtransformation
		{
		
		case 1:
			h=1;
			for (i=0;i<19;i++)
				TrafoPoint(hand[i], hand_n[i], winkel2[0]+180, hand[18], robo2_n[9]);
			break;
	
		case 2:
			h=0;
			for (i=0;i<19;i++)
				TrafoPoint(hand[i], hand_n[i], winkel2[0]+180, hand[18], robo2_n[9]);
			break;

		case 3:
			for (i=0;i<9;i++)
				TrafoPoint(schweisser[i], schweisser_n[i], winkel2[0]+180, schweisser[0], robo2_n[9]);
			break;

		} //switch(g)..

	
		if(f1==0)			//1. Blech im Greifer
			for (i=0;i<anz_p_b1;i++)
			{
				blech1_n[i][0] = blech1[i][0] + robo2_n[9][0];
				blech1_n[i][1] = blech1[i][1] + robo2_n[9][1];
			}
		
		else if (f1==1)		//1. Blech auf Eingangsfließband	
			for (i=0;i<anz_p_b1;i++)
				blech1_n[i][0] += t_step * 350 / (t_max);

	
		else if (f1==2)		//1. Blech auf Ausgangsfließband
			for (i=0;i<anz_p_b1;i++)			
				blech1_n[i][0] += t_step * 450 / (t_max);
						
			
		if(f2==0)			//2. Blech im Greifer
			for (i=0;i<anz_p_b2+anz_p_s;i++)
			{
				blech2_n[i][0] = blech2[i][0] + robo2_n[9][0];
				blech2_n[i][1] = blech2[i][1] + robo2_n[9][1];
			}
		
		else if (f2==1)		//2. Blech auf Eingangsfließband
			for (i=0;i<anz_p_b1;i++)
				blech2_n[i][0] += t_step * 350 / (t_max);
		
		else if (f2==2)		//2. Blech auf Ausgangsfließband
			for (i=0;i<anz_p_b2+anz_p_s;i++)
				blech2_n[i][0] += t_step * 450 / (t_max);
		
	
		

		InvalidateRect(hwnd, NULL, TRUE);
		hdc = BeginPaint (hwnd, &ps);
		 
		Zeichne_Statische_Elemente(hdc);
		Zeichne_Dynamische_Elemente(hdc);
		
		Sleep(t_step);


	} //for t..

} //void bewegen..
